#include "PumaQuat.h"

void PumaQuat::SetPumaPackets(const PumaPack& beg, const PumaPack& ending) {
	begin = beg;
	end =	ending;

	bquat[0].CreateFromAxisAngle(1.0f, .0f, .0f, begin.ang1);
	bquat[1].CreateFromAxisAngle(1.0f, .0f, .0f, begin.ang2);
	bquat[2].CreateFromAxisAngle(1.0f, .0f, .0f, begin.ang3);
	bquat[3].CreateFromAxisAngle(1.0f, .0f, .0f, begin.ang4);
	bquat[4].CreateFromAxisAngle(1.0f, .0f, .0f, begin.ang5);

	equat[0].CreateFromAxisAngle(1.0f, .0f, .0f, end.ang1);
	equat[1].CreateFromAxisAngle(1.0f, .0f, .0f, end.ang2);
	equat[2].CreateFromAxisAngle(1.0f, .0f, .0f, end.ang3);
	equat[3].CreateFromAxisAngle(1.0f, .0f, .0f, end.ang4);
	equat[4].CreateFromAxisAngle(1.0f, .0f, .0f, end.ang5);
}

void PumaQuat::SetPumaMatrix(const matrix44& beg, const matrix44& end) {
	Tbeg = beg; Tend = end;
	Qtbeg.CreateFromMatrix((float*)&Tbeg);
	Qtend.CreateFromMatrix((float*)&Tend);
}

//[ 0  1  2  3]
//[ 4  5  6  7]
//[ 8  9 10 11]
//[12 13 14 15]
matrix44 PumaQuat::CalculateMatrix44(float t) {
	Quaternion cur;
	matrix44 mat;
	vector3 d1, d2, d;

	d2.x = Tend[3][0]; d1.x = Tbeg[3][0];
	d2.y = Tend[3][1]; d1.y = Tbeg[3][1];
	d2.z = Tend[3][2]; d1.z = Tbeg[3][2];
	d = d1 + t* (d2 - d1);
	cur.QuatSlerp(Qtbeg, Qtend, t);
	cur.CreateMatrix((float*)&mat);
	//return TransposeMatrix44(mat);
	return TranslateMatrix44(d.x, d.y, d.z) * mat;
}

PumaPack PumaQuat::CalculatePumaPack(float t) {
	Quaternion	cur[5];
	float		curQ2;
	for(int i=0; i<5; ++i) {
		cur[i].QuatSlerp(bquat[i], equat[i], t);
	}
	curQ2 = begin.q2 + t *  (end.q2 - begin.q2);
	
	PumaPack pack;
	pack.rad = true;
	pack.q2 = curQ2;
	pack.ang1 = cur[0].ToAngleByXAxis();
	pack.ang2 = cur[1].ToAngleByXAxis();
	pack.ang3 = cur[2].ToAngleByXAxis();
	pack.ang4 = cur[3].ToAngleByXAxis();
	pack.ang5 = cur[4].ToAngleByXAxis();

	return pack;
}